#!/bin/bash

RED='\033[0;31m'
GREEN='\033[0;32m'
BLUE='\033[0;34m'
YELLOW='\033[0;33m'
NC='\033[0m' # 无颜色

# 提示用户准备开始执行标定程序
echo -e "${YELLOW}请确保 1、车辆所在地面水平 2、多线激光的配置已完成, 确定后点击 Y 进入多线激光标定程序${NC}"
read -p "输入 Y 继续: " user_input

# 检查用户输入是否为 Y
if [[ "$user_input" != "Y" ]]; then
    echo -e "${RED}未输入 Y，程序退出${NC}"
    exit 1
fi 

# 启动一个新的终端并打开fastlio程序
echo -e "${BLUE}正在启动新的终端并打开fastlio程序，请等待...${NC}"
gnome-terminal -- bash -c "source ADS_Proj/ADS_device/livox/install/setup.bash --extend; roslaunch livox_ros_driver2 msg_MID360.launch; exec bash"
sleep 5
gnome-terminal -- bash -c "source ADS_Proj/ADS_localization/install/setup.bash --extend; roslaunch fast_lio mapping_mid360.launch; exec bash"

# 等待5秒，等待程序启动
sleep 5

# 执行标定程序
./cal_xunluo

# 检测指定文件中是否有“标定完毕”
calibration_file="/mnt/autodrive/config/calibration/lidar_param.yaml"  # 标定状态保存在这个文件中
if grep -q "已经自动标定完成" "$calibration_file"; then
    echo -e "${GREEN}自动标定完成${NC}"
else
    echo -e "${RED}标定失败 请先检查多线激光的连接以及配置情况，确认无误后再次标定，如果依然标定失败，请联系技术人员${NC}"
fi

